/**
 * @file           : LKMotors235.h
 * @author         : Star_Plucking
 * @brief          : None
 * @date           : 25-1-11 上午12:04
 * @lastEditor     :
 * @lastEditTime   : 25-1-11 上午12:04
 */
#pragma once

#include "connection_guard.h"
#include <algorithm>
#include <sheriffos.h>

namespace device::lk_motors235 {
#define LK_CAN_ID 0x141

#define LK_CMD_READ_ENCODER 0x90
#define LK_CMD_READ_MULTI_ANGLE 0x92
#define LK_CMD_READ_STATE 0x9C
#define LK_CMD_CLOSE 0x80
#define LK_CMD_RUN 0x88
#define LK_CMD_OPEN_LOOP 0xA0
#define LK_CMD_TORQUE 0xA1
#define LK_CMD_SPEED 0xA2
#define LK_CMD_POSITION 0xA3
#define LK_CMD_POSITION_WITH_SPEED 0xA4
#define LK_CMD_CLEAR_FLAG 0x9B

enum class LKMotorType { MS, MF, MG };

enum class LKParamType { ANGLE, SPEED, IQ };

struct MotorInnerPID {
    uint16_t angle_kp{};
    uint16_t angle_ki{};
    uint16_t angle_kd{};
    uint16_t speed_kp{};
    uint16_t speed_ki{};
    uint16_t speed_kd{};
    uint16_t iq_kp{};
    uint16_t iq_ki{};
    uint16_t iq_kd{};
};
} // namespace device::lk_motors235

namespace device::lk_motors235 {
class Encoder {
private:
    float m_RawAngle = 0; ///< (单位：角度)电机编码器返回角0-360（不含角度偏移）
    float m_Speed = 0; ///< (单位：rpm)电机编码器返回速度
    float m_Current = 0; ///< (单位：A)电机编码器返回电流A
    float m_Power = 0; ///< (单位：W)电机编码器返回功率(仅MS系列）
    uint8_t m_Temp = 0; ///< (单位：摄氏度)电机编码器返回温度
    uint64_t m_LastUpdateTime = 0; ///< (单位：毫秒) 上次更新时间
    float m_MultiCircle = 0; ///< (单位：圈)电机多圈编码器返回角度

    int32_t m_Round_Count = 0; ///< 电机旋转圈数(含角度偏移)
    float m_ContinuousAngle = 0; ///< (单位：角度)电机连续角度(含角度偏移)
    float m_LimitedAngle = 0; ///< (单位：角度)电机当前角度-180-180(含角度偏移)
    float m_AngleOffset = 0; ///< (单位：角度)电机角度偏移

    float m_LastAngle = 0; ///< 上次角度(含角度偏移)
    float m_GearRatio = 0; ///< 减速比
    float m_ShaftSpeed = 0; ///< (单位：rpm)电机输出轴速度

    float kEncoderAngle = 0; ///< (单位：角度)编码器角度
    float kEncoderCurrentOrPower = 0;
    MotorInnerPID pid_param{};
    LKMotorType m_Type{};

public:
    Encoder() = default;
    /**
     * @param GearRatio 传动减速比
     * @param AngleOffset 角度偏置
     * @param BitCount 编码器位数
     * @param Type 电机类型
     * @note MG系列电机返回输出轴速度，MG系列直驱减速比应当设置为1
     */
    explicit Encoder(float const GearRatio, float const AngleOffset, uint8_t BitCount, LKMotorType Type)
        : m_AngleOffset(AngleOffset), m_GearRatio(GearRatio), m_Type(Type) {
        BitCount = std::min<int>(BitCount, 16);
        kEncoderAngle = 360.0F / static_cast<float>((1 << BitCount));
        switch (m_Type) {
            case LKMotorType::MG:
                kEncoderCurrentOrPower = 66.0F / 4096;
                break;
            case LKMotorType::MF:
                kEncoderCurrentOrPower = 33.0F / 4096;
                break;
            case LKMotorType::MS:
                kEncoderCurrentOrPower = 1;
            default:
                break;
        }
    }

    ~Encoder() = default;

    void state_decode(uint8_t const* data) {
        switch (data[0]) {
            case LK_CMD_READ_STATE:
            case LK_CMD_OPEN_LOOP:
            case LK_CMD_TORQUE:
            case LK_CMD_SPEED:
            case LK_CMD_POSITION:
            case LK_CMD_POSITION_WITH_SPEED:
                break;
            default:
                return;
        }

        m_LastAngle = m_LimitedAngle;
        // m_LastUpdateTime = units::unit_cast<uint64_t>(os::GetTime());
        m_RawAngle = kEncoderAngle * static_cast<float>((static_cast<uint16_t>(data[7]) << 8) | data[6]);
        m_Speed = static_cast<float>(
            static_cast<int16_t>(static_cast<uint16_t>(data[5] << 8) | static_cast<uint16_t>(data[4])));
        switch (m_Type) {
            case LKMotorType::MS:
                m_Power = kEncoderCurrentOrPower *
                          static_cast<float>(static_cast<int16_t>(static_cast<uint16_t>(data[3] << 8) |
                                                                  static_cast<uint16_t>(data[2])));
                break;
            case LKMotorType::MF:
            case LKMotorType::MG:
                m_Current = kEncoderCurrentOrPower *
                            static_cast<float>(static_cast<int16_t>(static_cast<uint16_t>(data[3] << 8) |
                                                                    static_cast<uint16_t>(data[2])));
                break;
            default:
                break;
        }

        m_Temp = data[1];

        if (float const tmpAngle = m_RawAngle + m_AngleOffset;
            tmpAngle > 180) {
            ///> 把LimitedAngle角度限制在-180-180
            m_LimitedAngle = tmpAngle - 360.0F;
        } else if (tmpAngle < -180.0F) {
            m_LimitedAngle = tmpAngle + 360.0F;
        } else {
            m_LimitedAngle = tmpAngle;
        }

        if (m_LimitedAngle - m_LastAngle < -160.0F) {
            m_Round_Count++;
        } else if (m_LimitedAngle - m_LastAngle > 160.0F) {
            m_Round_Count--;
        }
        m_ContinuousAngle = m_LimitedAngle + static_cast<float>(m_Round_Count) * 360.0F;

        m_ShaftSpeed = m_Speed / m_GearRatio;
    }

    void multiAngle_decode(uint8_t const* data) {
        switch (data[0]) {
            case LK_CMD_READ_MULTI_ANGLE:
                break;
            default:
                return;
        }
        int64_t temp_angle = 0;
        for (int i = 1; i < 8; i++) {
            temp_angle |= static_cast<int64_t>(data[i]) << (8 * (i - 1)); // 将每个 uint8_t 左移合并
        }
        m_MultiCircle = static_cast<float>(temp_angle) * 0.01F;

        // m_LastUpdateTime = units::unit_cast<uint64_t>(os::GetTime());
    }

    void decode(uint8_t const* data) {
        m_LastUpdateTime = units::unit_cast<uint64_t>(os::getTime());
        state_decode(data);
        multiAngle_decode(data);
    }

    [[nodiscard]] float GetCurrent() const { return m_Current; }

    [[nodiscard]] float GetLimitAngle() const { return m_LimitedAngle; }

    [[nodiscard]] float GetContinuesAngel() const { return m_ContinuousAngle; }

    [[nodiscaard]] float GetSpeed() const { return m_Speed; }

    [[nodiscard]] float GetPower() const { return m_Power; }

    [[nodiscard]] float GetTemp() const { return m_Temp; }

    [[nodiscard]] uint64_t GetLastUpdateTime() const { return m_LastUpdateTime; }

    [[nodiscard]] float GetMultiCircle() const { return m_MultiCircle; }

    [[nodiscard]] float GetGearRation() const { return m_GearRatio; }

    [[nodiscard]] MotorInnerPID GetPID() const { return pid_param; }

    [[nodiscard]] LKMotorType GetType() const { return m_Type; }
};
} // namespace device::lk_motors235

namespace device::lk_motors235 {
enum class LKMotorState { kEnable, kDisable, kLost };

enum class LKMotorMode { kTorque, kSpeed, kPosition };

class Motor {
private:
    LKMotorState m_State{};
    LKMotorState m_StareConnected{};
    LKMotorMode m_Mode{};
    float m_output = 0;
    uint32_t m_ID{};
    os::Can* m_can{};
    FDCAN_TxHeaderTypeDef _txHeader{
        .Identifier = m_ID,
        .IdType = FDCAN_STANDARD_ID,
        .TxFrameType = FDCAN_DATA_FRAME,
        .DataLength = FDCAN_DLC_BYTES_8,
        .ErrorStateIndicator = FDCAN_ESI_ACTIVE,
        .BitRateSwitch = FDCAN_BRS_OFF,
        .FDFormat = FDCAN_CLASSIC_CAN,
        .TxEventFifoControl = FDCAN_NO_TX_EVENTS,
        .MessageMarker = 0,
    };

public:
    Encoder encoder;
    Motor() = default;
    /**
     * @brief 瓴控电机构造函数
     * @param ID 电机ID
     * @param GearRatio 减速比(不包含电机减速箱)
     * @param AngleOffset 角度偏移
     * @param BitCount 编码器位数
     * @param Type 电机类型
     * @param can
     */
    explicit Motor(uint32_t const ID, float GearRatio, float const AngleOffset, uint8_t BitCount, LKMotorType Type,
                   os::Can* can)
        : m_ID(ID), m_can(can), encoder(GearRatio, AngleOffset, BitCount, Type) {
        m_State = LKMotorState::kLost; ///> 瓴控电机需要手动开启
        m_StareConnected = LKMotorState::kDisable;
        m_Mode = LKMotorMode::kTorque; ///> 默认为力矩模式
        os::ConnectionGuard::guard_().connect([this] { LostTest(); });
    }

    ~Motor() = default;

    uint32_t Get_ID() const {
        return m_ID;
    }

    void Enable() {
        uint8_t data[8] = {};
        data[0] = LK_CMD_RUN;
        if (m_State != LKMotorState::kLost) m_State = LKMotorState::kEnable;
        m_StareConnected = LKMotorState::kEnable;
        m_can->transmit(_txHeader, data);
    }

    void Disable() {
        uint8_t data[8] = {};
        data[0] = LK_CMD_CLOSE;
        if (m_State != LKMotorState::kLost) m_State = LKMotorState::kDisable;
        m_StareConnected = LKMotorState::kDisable;
        m_can->transmit(_txHeader, data);
    }

    void SetState(LKMotorState const state) { m_State = state; }

    [[nodiscard]] LKMotorState GetState() const { return m_State; }

    /**
     * @brief 设置电机输出角度闭环控制
     * @param angle 电机角度
     */
    void SetSendAngle(float angle) const {
        uint8_t data[8] = {};
        if (angle > 360) {
            angle = 360;
        } else if (angle < 0) {
            angle = 0;
        }
        angle *= encoder.GetGearRation();
        data[0] = LK_CMD_POSITION;
        auto angleControl = static_cast<int32_t>(angle * 100);
        // data[4] = angleControl & 0xFF;
        // data[5] = (angleControl >> 8) & 0xFF;
        // data[6] = (angleControl >> 16) & 0xFF;
        // data[7] = (angleControl >> 24) & 0xFF;
        data[4] = *(uint8_t*)(&angleControl);
        data[5] = *((uint8_t*)(&angleControl) + 1);
        data[6] = *((uint8_t*)(&angleControl) + 2);
        data[7] = *((uint8_t*)(&angleControl) + 3);
        m_can->transmit(_txHeader, data);
    }

    void SetSendAngleWithSpeed(float const angle, float const speed) const {
        uint8_t data[8] = {};
        data[0] = LK_CMD_POSITION_WITH_SPEED;
        auto angleControl = static_cast<int32_t>(angle / 100);
        data[4] = angleControl & 0xFF;
        data[5] = (angleControl >> 8) & 0xFF;
        data[6] = (angleControl >> 16) & 0xFF;
        data[7] = (angleControl >> 24) & 0xFF;
        auto speedControl = static_cast<uint16_t>(speed);
        data[2] = speedControl & 0xFF;
        data[3] = (speedControl >> 8) & 0xFF;
        m_can->transmit(_txHeader, data);
    }

    void SetSendSpeed(float const speed, float const torque) const {
        uint8_t data[8] = {};
        data[0] = LK_CMD_SPEED;
        auto speedControl = static_cast<int32_t>(speed * 100 * encoder.GetGearRation());
        data[4] = speedControl & 0xFF;
        data[5] = (speedControl >> 8) & 0xFF;
        data[6] = (speedControl >> 16) & 0xFF;
        data[7] = (speedControl >> 24) & 0xFF;
        auto torqueControl = static_cast<int16_t>(torque);
        if (torqueControl > 2048) {
            torqueControl = 2048;
        } else if (torqueControl < -2048) {
            torqueControl = -2048;
        }
        data[2] = torqueControl & 0xFF;
        data[3] = (torqueControl >> 8) & 0xFF;
        m_can->transmit(_txHeader, data);
    }

    /**
     * @brief 设置电机输出力矩控制
     * @param torque 力矩(-2048~2048)
     */
    void SetSendTorque(float const torque) {
        uint8_t data[8] = {};
        switch (encoder.GetType()) {
            case LKMotorType::MS: {
                data[0] = LK_CMD_OPEN_LOOP;
                auto powerControl = static_cast<int16_t>(torque);
                if (powerControl > 850) {
                    powerControl = 850;
                } else if (powerControl < -850) {
                    powerControl = -850;
                }
                data[4] = powerControl & 0xFF;
                data[5] = (powerControl >> 8) & 0xFF;
                break;
            }
            case LKMotorType::MF:
            case LKMotorType::MG: {
                data[0] = LK_CMD_TORQUE;
                auto torqueControl = static_cast<int16_t>(torque);
                if (torqueControl > 2048) {
                    torqueControl = 2048;
                } else if (torqueControl < -2048) {
                    torqueControl = -2048;
                }
                data[4] = torqueControl & 0xFF;
                data[5] = (torqueControl >> 8) & 0xFF;
                break;
            }
            default:
                break;
        }
        m_output = torque;
        m_can->transmit(_txHeader, data);
    }

    [[nodiscard]] float GetOutput() const { return m_output; }

    void Decode(uint16_t const ID, uint8_t const* data) {
        if (ID == m_ID) {
            encoder.decode(data);
        }
    }

    void GetSendMultiAngle() const {
        uint8_t data[8] = {};
        data[0] = LK_CMD_READ_MULTI_ANGLE;
        m_can->transmit(_txHeader, data);
    }

    void GetSendState() const {
        uint8_t data[8] = {};
        data[0] = LK_CMD_READ_STATE;
        m_can->transmit(_txHeader, data);
    }

    void ClearErrFlag() const {
        uint8_t data[8] = {};
        data[0] = LK_CMD_CLEAR_FLAG;
        m_can->transmit(_txHeader, data);
    }

    void LostTest() {
        if (HAL_GetTick() - encoder.GetLastUpdateTime() > 1000) {
            m_State = LKMotorState::kLost;
            m_StareConnected = LKMotorState::kDisable;
        } else {
            m_State = m_StareConnected;
        }
    }

    bool IsLost() const { return m_State == LKMotorState::kLost; }
};
} // namespace device::lk_motors235